#include "DSP2833x_Device.h"
#include "DSP2833x_GlobalPrototypes.h"
#include "System\SystemInit.h"
#include "DebugLED.h"
#include "UARTDriverA.h"
#include "UARTDriverB.h"
#include "GipsyModule.h"
#include "Timer.h"
#include "DataFile.h"
#include "PWMDriver.h"
#include "PWMDriver2.h"
#include "Alt\AltFilter.h"
#include "ADCDriver1.h"
#include "ADCDriver2.h"
#include "LevelDriver.h"
#include "ServoController.h"
#include "Processor.h"
#include "Parser\NmeaParser.h"
#include "Autobot\Auto.h"
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <deque>

#define LOCALSERIAL 1
#define M_PI       3.14159265358979323846f

UARTDriverA uartDriverA;
UARTDriverB uartDriverB;
GipsyModule gipsyModule;
PWMDriver pwmDriver;
PWMDriver2 pwmDriver2;
SDataFile df;
std::deque<char> g_Buffer;
void Process(char* buff, int size);
void ProcessReceive();
void ConvertToXY(long double longitude, long double latitude, float& x, float& y);
void KickTheDog(void);

// each tick is 20ms
#define SHUTDOWNTICKS 100

// main step time
#define STEPTIME 0.02

int g_SizeRec = 1; // num of received bytes from serial (used for serial debugging)

int main(void)
{
	SystemInit();
	SysInit();
	
	ADCDriver1 adcDriver1; // OK
	adcDriver1.Init();
	ADCDriver2 adcDriver2; // OK
	adcDriver2.Init();
	LevelDriver levelDriver;
	levelDriver.Init();
	Timer timer; // OK
	timer.Init();
	Timer timer4Hz;
	timer4Hz.BeginPeriod();
	DebugLED debugLED; // OK
	debugLED.Init();
	uartDriverA.Init(); // OK
	uartDriverA.SendData("BH Sensor v6.0, Build 500, DTW-X3 Edition\r\n");
	timer.WaitPeriod(1.0f); // wait for gypsy to stabilize after powerup
	gipsyModule.Initialize(); // OK?
	uartDriverB.Init(); // OK (XBEE)
	pwmDriver.Init(); // OK
	pwmDriver2.Init(); // OK
	g_Buffer.clear();
	ServoController servoCtrl;
	servoCtrl.Init();
	CProcessor processor;
	processor.Reset();
	NmeaParser* nmeaParser = NmeaParser::GetParser(); // create parser object
	
	// Auto-Controller
	Auto_initialize();

	// alt test
	AltFilter_initialize();
	
	df.MissionTime = 0; // reset mission time
	df.ComTXFails = 0; // reset error count
	df.State = 0;
	df.Altitude = 0;
	df.Latitude = 0;
	df.Longitude = 0;
	df.Servo1uS = 1000;
	df.Servo2uS = 1000;
	df.Servo3uS = 1000; 
	//df.Servo4uS = 1000; // servo 4 unused!
	df.Servo8Duty = 0;
	df.TailPotRefAngle = 0; // default angle (ZERO)
	df.FaileSafeShutdown = 0;
	memset( &df.Vars, 0, 4*30);
	Uint32 ledBlinkingCounter = 0;	
	
	// set parser default values
	//DTWX3Data& dD = nmeaParser->GetDTWX3Data();
	//dD.SetLeftMotor(df.Servo1uS);
	//dD.SetRightMotor(df.Servo2uS);
	//dD.SetRearMotor(df.Servo8Duty);
	//dD.SetRearServo(df.TailPotRefAngle);
	
	
	// ADC Warmup
	timer.BeginPeriod();
	while( !timer.HasElapsed(1.0) ) // 10ms period
	{
		adcDriver1.Update(); // pool adc
		adcDriver2.Update(); // pool adc
		levelDriver.Update();
	};

	
	// enable watchdog
	EALLOW;
    SysCtrlRegs.WDCR= 0x002F;
    EDIS;
	
	while(1)
	{	
		timer.BeginPeriod();
			
		// kick the dog
		KickTheDog();
			
		// Process GPS
		/*
		gipsyModule.Update();
		gipsyModule.GetTime(df.SatTime); 
		df.Longitude = gipsyModule.GetLongitude();
		df.Latitude = gipsyModule.GetLatitude();
		df.Altitude = gipsyModule.GetAltitude();
		df.SatNum = gipsyModule.GetSatUsed();
		*/
		
		// get data
		df.TailPotVolts = adcDriver2.GetValue(ADCDriver2::PotVoltage);
		
		
		df.Levels[0] = levelDriver.ReadAccel(LevelDriver::XAxis);
		df.Levels[1] = levelDriver.ReadAccel(LevelDriver::YAxis);
		df.Levels[2] = levelDriver.ReadAccel(LevelDriver::ZAxis);
		df.Rots[0] = adcDriver1.GetValue(ADCDriver1::RotX)+7.5f;
		df.Rots[1] = adcDriver1.GetValue(ADCDriver1::RotY)+5.26f;
		df.Rots[2] = adcDriver1.GetValue(ADCDriver1::RotZ)+10.1f;
		// Process processor :)
		processor.AddNewMeasurement(&df, STEPTIME); // takes around 5ms to execute! (debug/release)
		float roll, pitch, yaw;
		processor.GetRollPitchYaw(roll, pitch, yaw);
		df.RPY[0] = roll;
		df.RPY[1] = pitch;
		df.RPY[2] = yaw;
		
		// NEW parser
		char buffer[100];
#if LOCALSERIAL
		g_SizeRec = uartDriverA.GetData(buffer);
#else
		g_SizeRec = uartDriverB.GetData(buffer);
#endif
		nmeaParser->Parse(buffer, g_SizeRec);
		//DTWX3Data& dtwData = nmeaParser->GetDTWX3Data();
		PCX03Data& pcxData = nmeaParser->GetPCX03Data();
		AVX03Data& avxData = nmeaParser->GetAVX03Data();
		// auto shutdown
		if( g_SizeRec > 0 ) df.FaileSafeShutdown = SHUTDOWNTICKS; // have new data on serial port
		
		// Level1 Control
		Auto_U.RPY[0] = df.RPY[0]; // RPY in rads 
		Auto_U.RPY[1] = df.RPY[1];
		Auto_U.RPY[2] = df.RPY[2];
		Auto_U.omega[0] = -df.Rots[0] * M_PI / 180.0f; // Rots in deg/s (roll rate in wrong direction!)
		Auto_U.omega[1] = df.Rots[1] * M_PI / 180.0f;
		Auto_U.omega[2] = df.Rots[2] * M_PI / 180.0f;
		Auto_U.Roll = pcxData.GetJoystickX(); // Roll, Pitch, Yaw-> Joy values <-1, 1>
		Auto_U.Pitch = pcxData.GetJoystickY();
		Auto_U.Yaw = pcxData.GetJoystickZ();
		Auto_U.Thr = pcxData.GetThrottle();
		Auto_step();
		df.Servo1uS = Auto_Y.Left11001350;
		df.Servo2uS = Auto_Y.Right11001350;
		df.Servo3uS = Auto_Y.Tail11001600;
		df.TailPotRefAngle = pcxData.GetJoystickZ()*45;
		//df.TailPotRefAngle = 0;//Auto_Y.TailVec4545;
		
		/////////////////////////////
		// TESTING
		////////////////////////////	
		// Manual TESTs
		/*df.TailPotRefAngle = 0; // kill tail motor
		df.Servo4uS = 0;
		df.Servo1uS = dtwData.GetLeftMotor();
		df.Servo2uS = dtwData.GetRightMotor();
		*/
		// dump telemetry
		// MissionTime, Loop, M1, M2, OmegaX, Roll
		//sprintf(buffer, "%0.3Lf,%0.3f,%d,%d,%0.3f,%0.3f, %0.3f\r\n", df.MissionTime, df.LoopTime, df.Servo1uS, df.Servo2uS, -df.Rots[0], df.RPY[0], Auto_U.Roll );
		//uartDriverA.QueueData(buffer);
		avxData.SetRoll(df.RPY[0]);
		avxData.SetPitch(df.RPY[1]);
		avxData.SetYaw(df.RPY[2]);
		avxData.SetReserve1(df.Servo1uS);
		avxData.SetReserve2(df.Servo2uS);
		avxData.SetReserve3(df.Rots[0]);
		std::string str;
		//avxData.GenerateSentence(str);
		//uartDriverA.QueueData((char*)str.c_str());
		
		// Servo Control
		servoCtrl.SetPos(df.TailPotRefAngle);
		
		// failsafe
		if( df.FaileSafeShutdown > 0 )
		{
			df.FaileSafeShutdown--;
		}
		else
		{
			// TODO: slow shutdown when connection is lost!!!
			df.Servo1uS = 1000;
			df.Servo2uS = 1000;
			df.Servo3uS = 1000;
			//df.Servo5Duty = 0; // do not touch servo!!!
			//df.Servo6Duty = 0;
			//df.Servo7Duty = 0;
			df.Servo8Duty = 0;
		}
		
		// Set PWMs
		pwmDriver.SetPWMuSec(0, df.Servo1uS); // LEFT ESC
		pwmDriver.SetPWMuSec(1, df.Servo2uS); // RIGHT ESC
		pwmDriver.SetPWMuSec(2, df.Servo3uS); // Tail-PWM
		//pwmDriver.SetPWMuSec(3, df.Servo4uS); // N/A
		pwmDriver2.SetPWM(0, df.Servo5Duty); // Servo-EN (calculated in servoCtrl.SetPos() ) 
		pwmDriver2.SetPWM(1, df.Servo6Duty); // Servo-IN1 (calculated in servoCtrl.SetPos() ) 
		pwmDriver2.SetPWM(2, df.Servo7Duty); // Servo-IN2 (calculated in servoCtrl.SetPos() ) 
		pwmDriver2.SetPWM(3, df.Servo8Duty); // Light Show
			
		// blink led
		if( ++ledBlinkingCounter >= 10 )
		{
			debugLED.Toggle();
			ledBlinkingCounter = 0;
		}
		
		
		// update ADC
		adcDriver1.Update(); // do once to get valid looptime
		adcDriver2.Update();	
		levelDriver.Update(); // single update is enough
		df.LoopTime = timer.ElapsedTime(); // get looptime!		
		while( !timer.HasElapsed(STEPTIME) ) // 20ms period
		{
			adcDriver1.Update(); // pool adc
			adcDriver2.Update(); // pool adc
		};
		df.MissionTime += timer.ElapsedTime();		
	}
}


void KickTheDog(void)
{
    EALLOW;
    SysCtrlRegs.WDKEY = 0x0055;
    SysCtrlRegs.WDKEY = 0x00AA;
    EDIS;
}
